#include <stdio.h>
#include <stdlib.h>
#include <stdint.h>
#include <memory.h>

#include "ohos_init.h"
#include "cmsis_os2.h"
#include "iot_gpio.h"
#include "hi_io.h"
#include "hi_time.h"
#include "iot_i2c.h"
#include "iot_watchdog.h"
#include "iot_errno.h"
#include "mpuiic.h"
#include "common.h"

#define MPU_I2C_BAUDRATE (400*1000)
#define GPIO3 3
#define GPIO4 4
#define FUNC_SDA 6
#define FUNC_SCL 6
#define I2C1 1

#define MPU_I2C_IDX 0
#define MPU_WIDTH    (128)
#define MPU_I2C_ADDR 0x78 // 默认地址为 0x78
#define MPU_I2C_CMD 0x00 // 0000 0000       写命令
#define MPU_I2C_DATA 0x40 // 0100 0000(0x40) 写数据

typedef struct {
    /** Pointer to the buffer storing data to send */
    unsigned char *sendBuf;
    /** Length of data to send */
    unsigned int  sendLen;
    /** Pointer to the buffer for storing data to receive */
    unsigned char *receiveBuf;
    /** Length of data received */
    unsigned int  receiveLen;
} IotI2cData;
//IIC写数据 
//reg:寄存器地址
//data:数据
//返回值:0,正常
//    其他,错误代码
static uint32_t I2cWiteByte(uint8_t regAddr, uint8_t byte)
{
    unsigned int id = MPU_I2C_IDX;
    uint8_t buffer[] = {regAddr, byte};
    IotI2cData i2cData = {0};

    i2cData.sendBuf = buffer;
    i2cData.sendLen = sizeof(buffer) / sizeof(buffer[0]);

    return IoTI2cWrite(id, MPU_I2C_ADDR, i2cData.sendBuf, i2cData.sendLen);
}
//IIC读数据
//reg:寄存器地址 
//返回值:读到的数据
static uint32_t I2cReadByte(uint8_t regAddr, uint8_t byte)
{
    unsigned int id = MPU_I2C_IDX;
    uint8_t buffer[] = {regAddr, byte};
    IotI2cData i2cData = {0};

    i2cData.receiveBuf = buffer;
    i2cData.receiveLen = sizeof(buffer) / sizeof(buffer[0]);

    return IoTI2cRead(id, MPU_I2C_ADDR, i2cData.receiveBuf, i2cData.receiveLen);
}

//IIC连续写
//addr:器件地址 
//reg:寄存器地址
//len:写入长度
//buf:数据区
//返回值:0,正常
//    其他,错误代码
static uint32_t I2cWiteLen(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf)
{
    unsigned int id = MPU_I2C_IDX;
    uint8_t buffer[] = {reg, len};
    IotI2cData i2cData = {0};

    i2cData.sendBuf = buffer;
    i2cData.sendLen = sizeof(buffer);

    return IoTI2cWrite(id, MPU_I2C_ADDR, i2cData.sendLen, i2cData.sendBuf);
    
}
//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
//    其他,错误代码
static uint32_t I2cReadLen(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf)
{
    unsigned int id = MPU_I2C_IDX;
    uint8_t buffer[] = {reg, len};
    IotI2cData i2cData = {0};

    i2cData.receiveBuf = buffer;
    i2cData.receiveLen = sizeof(buffer);

    return IoTI2cWrite(id, MPU_I2C_ADDR, i2cData.receiveBuf, i2cData.receiveLen);
    
}


/**
 * @brief Write a command byte to device.
 *
 * @param cmd the commnad byte to be writen.
 * @return Returns {@link IOT_SUCCESS} if the operation is successful;
 * returns an error code defined in {@link wifiiot_errno.h} otherwise.
 */
static uint32_t WriteCmd(uint8_t cmd)
{
    return I2cWiteByte(MPU_I2C_CMD, cmd);
}

/**
 * @brief Write a data byte to device.
 *
 * @param cmd the data byte to be writen.
 * @return Returns {@link IOT_SUCCESS} if the operation is successful;
 * returns an error code defined in {@link wifiiot_errno.h} otherwise.
 */
static uint32_t WriteData(uint8_t data)
{
    return I2cWiteByte(MPU_I2C_DATA, data);
}


//初始化IIC
void MPU_IIC_Init(void)
{
    hi_io_set_func(GPIO3, FUNC_SDA);
    hi_io_set_func(GPIO4, FUNC_SCL);
    IoTI2cInit(I2C1, MPU_I2C_BAUDRATE);
}


//设置MPU6050陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败
uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr)
{
	return I2cWiteByte(MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围  
}
//设置MPU6050加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
//    其他,设置失败 
uint8_t MPU_Set_Accel_Fsr(uint8_t fsr)
{
	return I2cWiteByte(MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围 
}
//设置MPU6050的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
//    其他,设置失败 
uint8_t MPU_Set_LPF(uint16_t lpf)
{
	uint8_t data=0;
	if(lpf>=188)data=1;
	else if(lpf>=98)data=2;
	else if(lpf>=42)data=3;
	else if(lpf>=20)data=4;
	else if(lpf>=10)data=5;
	else data=6; 
	return I2cWiteByte(MPU_CFG_REG,data);//设置数字低通滤波器  
}
//设置MPU6050的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
//    其他,设置失败 
uint8_t MPU_Set_Rate(uint16_t rate)
{
	uint8_t data;
	if(rate>1000)rate=1000;
	if(rate<4)rate=4;
	data=1000/rate-1;
	data=I2cWiteByte(MPU_SAMPLE_RATE_REG,data);//设置数字低通滤波器
 	return MPU_Set_LPF(rate/2);		//自动设置LPF为采样率的一半

}
//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{
    uint8_t buf[2]; 
    short raw;
	float temp;
	I2cReadLen(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf); 
    raw=((uint16_t)buf[0]<<8)|buf[1];  
    temp=36.53+((double)raw)/340;  
    return temp*100;;
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
uint8_t MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
    uint8_t buf[6],res;  
	res=I2cReadLen(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
	if(res==0)
	{
		*gx=((uint16_t)buf[0]<<8)|buf[1];  
		*gy=((uint16_t)buf[2]<<8)|buf[3];  
		*gz=((uint16_t)buf[4]<<8)|buf[5];
	} 	
    return res;;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
uint8_t MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
    uint8_t buf[6],res;  
	res=I2cReadLen(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
	if(res==0)
	{
		*ax=((uint16_t)buf[0]<<8)|buf[1];  
		*ay=((uint16_t)buf[2]<<8)|buf[3];  
		*az=((uint16_t)buf[4]<<8)|buf[5];
	} 	
    return res;;
}

uint8_t MPU_Init(void)
{ 
	uint8_t res; 
	MPU_IIC_Init();//初始化IIC总线
	I2cWiteByte(MPU_PWR_MGMT1_REG,0X80);	//复位MPU6050
    usleep(100);                            //延遲
	I2cWiteByte(MPU_PWR_MGMT1_REG,0X00);	//唤醒MPU6050 
	MPU_Set_Gyro_Fsr(3);				//陀螺仪传感器,±2000dps
	MPU_Set_Accel_Fsr(0);					//加速度传感器,±2g
	MPU_Set_Rate(200);						
	I2cWiteByte(MPU_INT_EN_REG,0X00);	
	I2cWiteByte(MPU_USER_CTRL_REG,0X00);	
	I2cWiteByte(MPU_FIFO_EN_REG,0X00);
	I2cWiteByte(MPU_INTBP_CFG_REG,0X80);	
	if(res==MPU_ADDR)
	{
		I2cWiteByte(MPU_PWR_MGMT1_REG,0X01);
		I2cWiteByte(MPU_PWR_MGMT2_REG,0X00);	
		MPU_Set_Rate(200);					
 	}else return 1;
	return 0;
}